#include "kf.h"

SysModel::SysModel()
{

}

SysModel::SysModel(MatrixXd state, MatrixXd stateCov, MatrixXd q)
{
    mState = state;
    mStateCov = stateCov;
    mq = q;
}
SysModel::~SysModel()
{

}

MatrixXd SysModel::f(double delteT)
{
    MatrixXd F(3,3);
    F<<1,delteT,delteT*delteT*0.5,0,1,delteT,0,0,1;
    return F;
}

MatrixXd SysModel::Q(double delteT)
{
    MatrixXd gamma(3,1);
    gamma<<delteT*delteT*0.5, delteT, 1;
    return gamma*q()*gamma.transpose();
}

MatrixXd& SysModel::state()
{
    return mState;
}

MatrixXd& SysModel::stateCov()
{
    return mStateCov;
}

MatrixXd& SysModel::q()
{
    return mq;
}

MeasureModel::MeasureModel(){}

MeasureModel::MeasureModel(MatrixXd R)
{
    mR = R;
}

MeasureModel::~MeasureModel()
{}

MatrixXd& MeasureModel::R()
{
    return mR;
}

MatrixXd MeasureModel::h()
{
    MatrixXd H(1,3);
    H<<1,0,0;
    return H;
}


KF::KF(SysModel sys, MeasureModel measure)
{
    mSys = sys;
    mMeasure = measure;
}

KF::~KF()
{

}
void KF::addZk(MatrixXd z, double delteT)
{


    //新的量测值Z_1
    MatrixXd Zk1 = z;

    //k时刻的状态转移矩阵
    MatrixXd Fk = mSys.f(delteT);

    MatrixXd Hk = mMeasure.h();

    MatrixXd Rk = mMeasure.R();

    MatrixXd Qk = mSys.Q(delteT);

    MatrixXd Xk = mSys.state();

    MatrixXd Pkk = mSys.stateCov();

    //状态的一步预测
    MatrixXd X_k1k = Fk*Xk;

    //一步预测协方差,P(k+1|k)=F(k)P(k|k)F'(k) + Q(k)
    //一步预测P(k+1|k)为对称阵，它可用来衡量预测的不确定性，P(k+1|k)越小则预测越精确
    MatrixXd Pk1k = Fk*Pkk*Fk.transpose() + Qk;

    //量测的预测,Z_(k+1|k)=H(k+1)X_(k+1|k)
    MatrixXd Z_k1k = Hk*X_k1k;

    //量测的预测值和量测值之间的差,Z_diff(k+1|k)=Z(k+1)-Z_(k+1|k)
    MatrixXd ZDiffK1k = Zk1 - Z_k1k;

    //量测的预测协方差（或新息协方差）,S(k+1)=H(k+1)P(k+1|k)H'(k+1)+R(k+1)
    //新息协方差S(k+1)为对称阵，它是用来衡量新息的不确定性，新息协方差越小，则说明量测值越精确
    MatrixXd Sk1 = Hk*Pk1k*Hk.transpose() + Rk;

    //状态和量测之间的协方差,Pxz=P(k+1|k)H'(k+1)
    MatrixXd Pxz = Pk1k*Hk.transpose();

    //增益，增益的大小反映了最新观测 信息对状态估计量的贡献大小,K(k+1)=P(k+1|k)H'(k+1)S_1(k+1)
    MatrixXd Kk1 = Pk1k*Hk.transpose()*Sk1.inverse();

    //量测残差γ(k+1)=Z_diff(k+1|k)
    mGammak = ZDiffK1k;

    //k+1时刻的状态更新方程,X_(k+1|k+1)=X_(k+1|k)+K(k+1)γ(k+1)
    //k+1时刻的估计X_(k+1|k+1)等于该时刻的状态预测值X_(k+1|k)再加上一个修正项，而这个修正项与增益K(k+1)和新息有关
    MatrixXd X_k1k1 = X_k1k + Kk1*mGammak;
//    std::cout<<Pk1k<<std::endl;
    //协方差更新方程，P(k+1|k+1)=P(k+1|k)-P(k+1|k)H'(k+1)S_1(k+1)H(k+1)P(k+1|k)
    MatrixXd Pk1k1 = Pk1k - Pk1k*Hk.transpose()*Sk1.inverse()*Hk*Pk1k;

    mSys.stateCov() = Pk1k1;
    mSys.state() = X_k1k1;

}
